#include "opengl.h"
#include "camera.h"
#include "unistate.h"
#include "vr.h"

using namespace goatgfx;

Camera::Camera()
{
	roll = 0.0;
	inval_cache();
}

Camera::~Camera()
{
}

void Camera::calc_inv_matrix(Matrix4x4 *mat) const
{
	*mat = matrix().inverse();
}

const Matrix4x4 &Camera::matrix() const
{
	if(!mcache.valid) {
		calc_matrix(&mcache.mat);
		mcache.valid = true;
	}
	return mcache.mat;
}

const Matrix4x4 &Camera::inv_matrix() const
{
	if(!mcache_inv.valid) {
		calc_inv_matrix(&mcache_inv.mat);
		mcache_inv.valid = true;
	}
	return mcache_inv.mat;
}

void Camera::use() const
{
	set_view_matrix(matrix());
}

void Camera::use_inverse() const
{
	set_view_matrix(inv_matrix());
}

void Camera::input_move(float x, float y, float z)
{
}

void Camera::input_rotate(float x, float y, float z)
{
}

void Camera::input_zoom(float factor)
{
}


// ---- orbit camera ----

OrbitCamera::OrbitCamera()
{
	theta = 0.0;
	phi = 0.0;
	rad = 10.0;
}

OrbitCamera::~OrbitCamera()
{
}

void OrbitCamera::calc_matrix(Matrix4x4 *mat) const
{
	mat->reset_identity();
	mat->translate(Vector3(0, 0, -rad));
	mat->rotate(Vector3(phi, 0, 0));
	mat->rotate(Vector3(0, theta, 0));
	mat->rotate(Vector3(0, 0, roll));
}

void OrbitCamera::calc_inv_matrix(Matrix4x4 *mat) const
{
	mat->reset_identity();
	mat->rotate(Vector3(0, 0, roll));
	mat->rotate(Vector3(0, theta, 0));
	mat->rotate(Vector3(phi, 0, 0));
	mat->translate(Vector3(0, 0, -rad));
}

void OrbitCamera::input_rotate(float x, float y, float z)
{
	theta += y;
	phi += x;
	roll += z;

	if(phi < -M_PI / 2)
		phi = -M_PI / 2;
	if(phi > M_PI)
		phi = M_PI;

	inval_cache();
}

void OrbitCamera::input_zoom(float factor)
{
	rad += factor;
	if(rad < 0.0)
		rad = 0.0;

	inval_cache();
}

void FpsCamera::calc_matrix(Matrix4x4 *mat) const
{
	mat->reset_identity();
	mat->translate(Vector3(pos.x, pos.y, pos.z));
	mat->rotate(Vector3(0, theta, 0));
	mat->rotate(Vector3(phi, 0, 0));
	mat->rotate(Vector3(0, 0, roll));
}

void FpsCamera::calc_inv_matrix(Matrix4x4 *mat) const
{
	mat->reset_identity();
	mat->rotate(Vector3(0, 0, roll));
	mat->rotate(Vector3(phi, 0, 0));
	mat->rotate(Vector3(0, theta, 0));
	mat->translate(Vector3(-pos.x, -pos.y, -pos.z));
}

void FpsCamera::input_move(float x, float y, float z)
{
	pos.x += x * cos(theta) - z * sin(theta);
	pos.z += x * sin(theta) + z * cos(theta);
	pos.y += y;
	inval_cache();
}

const Vector3 &FpsCamera::get_position() const
{
	return pos;
}


FlyCamera::FlyCamera()
{
	pos.z = 10.0f;
}

void FlyCamera::calc_matrix(Matrix4x4 *mat) const
{
	Matrix3x3 rmat = rot.get_rotation_matrix().transposed();
	Matrix4x4 tmat;
	tmat.set_translation(pos);
	*mat = tmat * Matrix4x4(rmat);
}

/*void FlyCamera::calc_inv_matrix(Matrix4x4 *mat) const
{
}*/

const Vector3 &FlyCamera::get_position() const
{
	return pos;
}

const Quaternion &FlyCamera::get_rotation() const
{
	return rot;
}

void FlyCamera::input_move(float x, float y, float z)
{
	static const Vector3 vfwd(0, 0, 1), vright(1, 0, 0);

	Vector3 k = vfwd.transformed(rot);
	Vector3	i = vright.transformed(rot);
	Vector3 j = cross_product(k, i);

	pos += i * x + j * y + k * z;
	inval_cache();
}

void FlyCamera::input_rotate(float x, float y, float z)
{
	Vector3 axis(x, y, z);
	float axis_len = axis.length();
	rot.rotate(axis / axis_len, axis_len);
	rot.normalize();

	inval_cache();
}


// --- VR additions ---
VRFpsCamera::VRFpsCamera()
{
	neck_eye_dist = 0.14;	// default neck->eye distance 14cm
}

void VRFpsCamera::calc_matrix(Matrix4x4 *mat) const
{
	mat->reset_identity();
	mat->translate(Vector3(pos.x, pos.y, pos.z));
	mat->rotate(Vector3(0, theta, 0));
	mat->rotate(Vector3(phi, 0, 0));
	mat->rotate(Vector3(0, 0, roll));
	mat->translate(Vector3(0, neck_eye_dist, 0));
}

void VRFpsCamera::calc_inv_matrix(Matrix4x4 *mat) const
{
	mat->reset_identity();
	mat->translate(Vector3(0, -neck_eye_dist, 0));
	mat->rotate(Vector3(0, 0, roll));
	mat->rotate(Vector3(phi, 0, 0));
	mat->rotate(Vector3(0, theta, 0));
	mat->translate(Vector3(-pos.x, -pos.y, -pos.z));
}

void VRFpsCamera::track_vr()
{
	float euler[3];
	vr_get_rotation_euler(euler);

	// input_rotate invalidates cache
	input_rotate(prev_angles[0] - euler[0], prev_angles[1] - euler[1], prev_angles[2] - euler[2]);

	prev_angles[0] = euler[0];
	prev_angles[1] = euler[1];
	prev_angles[2] = euler[2];
}

